import os
import halcon as ha
from  util_3d  import *
class Camera():

    def __init__(self):
        self.acq_handle = ha.open_framegrabber('MVision', 1, 1, 0, 0, 0, 0, 'progressive', 8, 'default', -1, 'false', 'auto',
                          'U3V:DA3797129 MV-CU060-10UM', 0, -1)

        ha.set_framegrabber_param(self.acq_handle, 'AcquisitionFrameRate', 10.0)
        ha.set_framegrabber_param(self.acq_handle, 'AcquisitionFrameRateEnable', 1)
        # self.set_param()
        ha.grab_image_start(self.acq_handle, -1)
        self.Image = ha.grab_image_async(self.acq_handle, -1)
        Width, Height = ha.get_image_size(self.Image)
        self.window_image = self.open_window(Width[0]/4, Height[0]/4)
        ha.disp_obj(self.Image, self.window_image)

    def update_image(self, disp_flag):
        try:
            self.Image = ha.grab_image(self.acq_handle)
            update_flag = True
        except:
           ha.close_framegrabber(self.acq_handle)
           update_flag = False
           try:
               self.acq_handle = ha.open_framegrabber('MVision', 1, 1, 0, 0, 0, 0, 'progressive', 8, 'default', -1, 'false', 'auto',
                                                      'U3V:DA3797129 MV-CU060-10UM', 0, -1)
               ha.set_framegrabber_param(self.acq_handle, 'AcquisitionFrameRate', 10.0)
               ha.set_framegrabber_param(self.acq_handle, 'AcquisitionFrameRateEnable', 1)
               ha.grab_image_start(self.acq_handle, -1)
           except:
               print('error open camera')

        if disp_flag == True:
            ha.disp_obj(self.Image, self.window_image)
        return self.Image, update_flag

    def get_image(self):
        return self.Image

    def update_image_loop(self):
        while True:
            try:
                self.Image = ha.grab_image(self.acq_handle)
                ha.disp_obj(self.Image, self.window_image)
            except:
                ha.close_framegrabber(self.acq_handle)
                try:
                    self.acq_handle = ha.open_framegrabber('MVision', 1, 1, 0, 0, 0, 0, 'progressive', 8, 'default', -1, 'false', 'auto',
                                                           'U3V:DA3797129 MV-CU060-10UM', 0, -1)
                    ha.set_framegrabber_param(self.acq_handle, 'AcquisitionFrameRate', 10.0)
                    ha.set_framegrabber_param(self.acq_handle, 'AcquisitionFrameRateEnable', 1)
                    ha.grab_image_start(self.acq_handle, -1)
                except:
                    print('error open camera')

    def open_window(self, width, height, row=0, col=0, win_id=0):
        if os.name == 'nt':
            ha.set_system('use_window_thread', 'true')
        window = ha.open_window(
            row=row,
            column=col,
            width=width,
            height=height,
            father_window=win_id,
            mode='visible',
            machine=''
        )
        ha.set_draw(window, 'fill')
        ha.set_line_width(window, 2)
        ha.set_color(window, 'green')
        return window

    def set_param(self):
        self.CamParam = ha.read_cam_par('C:/Users/k/Desktop/标定/calib1.cal')
        self.CaltabName = 'caltab_30mm.descr'
        self.CalibDataID = ha.create_calib_data('calibration_object', 1, 1)
        ha.set_calib_data_cam_param(self.CalibDataID, 0, [], self.CamParam)
        ha.set_calib_data_calib_object(self.CalibDataID, 0, self.CaltabName)
        pass

    def set_calib_plate(self, param):
        pass
    def gen_calib_pos(self):
        try:
            ha.find_calib_object(self.Image, self.CalibDataID, 0, 0, 1, [], [])
            Caltab = ha.get_calib_data_observ_contours(self.CalibDataID, 'caltab', 0, 0, 1)
            RCoord, CCoord, Index, self.PoseForCalibrationPlate = ha.get_calib_data_observ_points(self.CalibDataID, 0, 0, 1)
            # ha.set_color(self.window_image, 'green')
            ha.disp_caltab(self.window_image, self.CaltabName, self.CamParam, self.PoseForCalibrationPlate,1)
            print(self.PoseForCalibrationPlate)
            return True, self.PoseForCalibrationPlate
        except:
            print('error')
            return False, self.PoseForCalibrationPlate

    def gen_cam_t_calib(self, type):
        self.PoseForCalibrationPlate = xyzrpy_to_T(0,0,0,0,0,0)
        for i in range(5):
            try:
                ha.find_calib_object(self.Image, self.CalibDataID, 0, 0, 1, [], [])
                Caltab = ha.get_calib_data_observ_contours(self.CalibDataID, 'caltab', 0, 0, 1)
                RCoord, CCoord, Index, self.PoseForCalibrationPlate = ha.get_calib_data_observ_points(self.CalibDataID, 0, 0, 1)
                # ha.set_color(self.window_image, 'green')
                ha.disp_caltab(self.window_image, self.CaltabName, self.CamParam, self.PoseForCalibrationPlate,1)
                if type == "halcon":
                    return True, self.PoseForCalibrationPlate
                else:
                    return True, halcon_pose_to_T(self.PoseForCalibrationPlate)
            except:
                if i == 4:
                    print('error')
                    return False, self.PoseForCalibrationPlate


if __name__ == '__main__':
    ca = Camera()
    ca.set_param()
    while True:
        img, update_flag = ca.update_image(True)
        ca.gen_calib_pos()

